#!/usr/bin/python
import roslib; roslib.load_manifest('DelphinROSv2')
import rospy
import time
import numpy
from std_msgs.msg import Float32
from std_msgs.msg import Int8
from DelphinROSv2.msg import compass as compass_out
from DelphinROSv2.msg import position
from DelphinROSv2.msg import gps

#### DEFINE GLOBAL VARIABLES ####
global flag
global cur_compass
global cur_sway
global cur_prop

########## LOW LEVEL CONTROL ############################################################
def reckoner():

    #### SETUP ####
        global cur_compass
        global cur_sway
        global cur_prop
        global X
        global Y
        global Z
        global flag
	global GPS_flag

        global GPS_X
        global GPS_Y

        flag = False
	GPS_flag=False
        cur_sway = Float32()
        cur_prop = Float32()
        cur_compass = compass_out()
        time_zero = time.time()	
       

        cur_sway.data = 0.0
        cur_prop.data = 0.0

	Tcx = 3.68 		# Time constant for X direction
	Tcy = 4

	Vx=0.0
	Vy=0.0

	X=0.0
	Y=0.0
	Z=0.0
	GPS_X=0
	GPS_Y=0

        print 'Entering main loop!!!'
        #### MAIN RECKONER LOOP ####
        while not rospy.is_shutdown():
	    if GPS_flag==True:
		X=GPS_X
		Y=GPS_Y
		Z=cur_compass.depth
 		GPS_flag=False
            elif flag == True:

		#### BASIC CALCS ####
                dt = time.time() - time_zero	                # Calculate dt time between last loop interation
                time_zero = time.time()		                # Record time of start of this loop
                print 'Basic calcs done'

		#### Z POSITION ####
                Z=cur_compass.depth
                print 'Z position done'

		#### FORWARD VEL ####
                prop = float(cur_prop.data)
		Vx_ss = -0.0004809*prop**2 + 0.0567*prop	# Calculate steady state forward velocity
                if prop < 0:                                    # If prop demand is in reverse then half the vel setpoint
                    Vx_ss = Vx_ss / 2.0                
		Vx_error = Vx_ss - Vx				# Difference between current vel estimate and steady state
		Vx = Vx+Vx_error*(dt/Tcx)			# First order approximation
                print 'Forward velocity done'
                
		#### SWAY VEL ####
		Vy_ss = float(cur_sway.data)/10000       			# Same as for forward vel section above
		Vy_error = Vy_ss - Vy
		Vy = Vy+Vy_error*(dt/Tcy)
                print 'Sway velocity done'
	
 		#### GET HEADING ####
		heading=cur_compass.heading
    	        print 'heading', heading
		heading= heading/180*numpy.pi

                #### XY VELOCITIES ####
    	        u=Vx*numpy.cos(heading)-Vy*numpy.sin(heading)
    	        v=Vx*numpy.sin(heading)+Vy*numpy.cos(heading)
	
		#### XY LOCATION ####
		X=X+u*dt
		Y=Y+v*dt
	
		#### PRINT DATA ####
    	        print 'u', u
    	        print 'v', v
	
    	        print 'X', X
    	        print 'Y', Y
    	        print 'Z', Z
	
		#### PUBLISH LOCATATION ####
		output=position()
    	        output.X = X
    	        output.Y = Y
    	        output.Z = Z
                output.forward_vel = Vx
                output.sway_vel = Vy
		pub.publish(output)

		flag=False
		#### END OF MAIN DEAD RECKONING LOOP ####

######## END DEAD RECKONER #####################################################


#### WHEN NEW MISSION OR COMPASS DATA ARRIVES UPDATE VALUES AND RESET FLAG ####

def compass_cb(compass):
    global flag
    global cur_compass
    cur_compass = compass   
    print 'new compass data!!' 
    flag = True

def prop_cb(prop):
    global flag
    global cur_prop
    cur_prop = prop   
    print 'new prop data!!' 
    flag = True

def sway_cb(sway):
    global flag
    global cur_sway
    cur_sway = sway
    print 'new sway data!!' 
    flag = True

def gps_callback(gps):
    global GPS_flag
    global GPS_X
    global GPS_Y
    print 'new GPS data'
    GPS_flag=True
    GPS_X=gps.x
    GPS_Y=gps.y

#### INITIALISATION ####
if __name__ == '__main__':
    rospy.init_node('dead_reckoner')
    
    rospy.Subscriber('compass_out', compass_out, compass_cb)
    rospy.Subscriber('prop_demand',Int8, prop_cb)
    rospy.Subscriber('sway_demand',Float32, sway_cb)
    rospy.Subscriber('gps_out', gps, gps_callback)
    pub = rospy.Publisher('position_dead', position)

    reckoner()
